/*
	TOPCAT Payload Software, for use in the UKube-1 CubeSat mission.
	
	http://www.bath.ac.uk/elec-eng/invert/topcat.html
	
	Written by:
	Mark Jessop, University of Adelaide
	Talini Pinto Jayawardena, University of Bath
	
	Uses extracts from source code provided by Clyde Space for I2C communications.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    For a full copy of the GNU General Public License, 
    see <http://www.gnu.org/licenses/>.
*/

/////////////////////////
// I2C Command Handlers.
/////////////////////////


void pay_ops_initialise(unsigned int crc){
	#if OLD_ACK == 1
	I2C_send_crc_ack(crc);
	#else
	I2C_send_ack(0x90);
	#endif
	
	// Extract data from the packet. The increment by OLD_ACK allows implementation of both
	// old and new payload specs simultaneously.
	#if OLD_ACK == 1
	PAYLOAD_OP_MODE = i2c_rxd_buffer[2];	
	#endif
	
	PAYLOAD_OP_FLAGS = i2c_rxd_buffer[2 + OLD_ACK];
	PAYLOAD_OP_FLAGS = PAYLOAD_OP_FLAGS<<8;
	PAYLOAD_OP_FLAGS |= i2c_rxd_buffer[3 + OLD_ACK];
	
	PAYLOAD_PRIORITY_DATA_LIMIT = i2c_rxd_buffer[8 + OLD_ACK];
	PAYLOAD_PRIORITY_DATA_LIMIT = PAYLOAD_PRIORITY_DATA_LIMIT << 8;
	PAYLOAD_PRIORITY_DATA_LIMIT |= i2c_rxd_buffer[9 + OLD_ACK];
	
	PAYLOAD_PRIORITY_DATA_REMAINING = i2c_rxd_buffer[10 + OLD_ACK];
	PAYLOAD_PRIORITY_DATA_REMAINING = PAYLOAD_PRIORITY_DATA_REMAINING << 8;
	PAYLOAD_PRIORITY_DATA_REMAINING |= i2c_rxd_buffer[11 + OLD_ACK];
	
	PAYLOAD_MASS_MEMORY_LIMIT = i2c_rxd_buffer[12 + OLD_ACK];
	PAYLOAD_MASS_MEMORY_LIMIT = PAYLOAD_MASS_MEMORY_LIMIT << 8;
	PAYLOAD_MASS_MEMORY_LIMIT |= i2c_rxd_buffer[13 + OLD_ACK];
	PAYLOAD_MASS_MEMORY_LIMIT = PAYLOAD_MASS_MEMORY_LIMIT << 8;
	PAYLOAD_MASS_MEMORY_LIMIT |= i2c_rxd_buffer[14 + OLD_ACK];
	PAYLOAD_MASS_MEMORY_LIMIT = PAYLOAD_MASS_MEMORY_LIMIT << 8;
	PAYLOAD_MASS_MEMORY_LIMIT |= i2c_rxd_buffer[15 + OLD_ACK];
	
	INIT_PAYLOAD_MASS_MEMORY_REMAINING = i2c_rxd_buffer[16 + OLD_ACK];
	INIT_PAYLOAD_MASS_MEMORY_REMAINING = PAYLOAD_MASS_MEMORY_REMAINING << 8;
	INIT_PAYLOAD_MASS_MEMORY_REMAINING |= i2c_rxd_buffer[16 + OLD_ACK];
	INIT_PAYLOAD_MASS_MEMORY_REMAINING = PAYLOAD_MASS_MEMORY_REMAINING << 8;
	INIT_PAYLOAD_MASS_MEMORY_REMAINING |= i2c_rxd_buffer[18 + OLD_ACK];
	INIT_PAYLOAD_MASS_MEMORY_REMAINING = PAYLOAD_MASS_MEMORY_REMAINING << 8;
	INIT_PAYLOAD_MASS_MEMORY_REMAINING |= i2c_rxd_buffer[19 + OLD_ACK];
	
    oemv_startup(); // Clear all GPS Flags, startup GPS.
    gps_startup_timer = millis();
    Serial.println("GPS Powered Up");
    
    PAYLOAD_MASS_MEMORY_REMAINING = INIT_PAYLOAD_MASS_MEMORY_REMAINING;
    ORBIT_MAX_MEM = (30*1024)/256;  //Initialize to 30KB then change as needed on mode change or parameter write
    PAYLOAD_INITIALIZED = 1; // Notify the main loop that we are initialized.
}

void pay_ops_status(){
	unsigned int crc;
	
	// Construct Status Packet in I2C TX Buffer.
	i2c_txd_buffer[0] = 0x91;
	
	if(ERROR_FLAGS != 0){ // Something has raised an error flag. Send an error packet instead of a status update.
		i2c_txd_buffer[1] = 0x09; // CRC bit is always set, and we have an error, hence 0b1001.
		i2c_txd_buffer[2] = ERROR_FLAGS;
		crc = crc16(0xFFFF,i2c_txd_buffer,3);
		i2c_txd_buffer[3] = (uint8_t)(crc>>8);
		i2c_txd_buffer[4] = (uint8_t)(crc&0xFF);
		i2c_tx_length = 5;
	}else{
	
		unsigned long data_waiting_temp = (unsigned long)PAYLOAD_DATA_WAITING * 256;
		i2c_txd_buffer[1] = 0x01;
		i2c_txd_buffer[2] = PAYLOAD_OP_MODE;
		i2c_txd_buffer[3] = (uint8_t)(PAYLOAD_OP_FLAGS>>8);
		i2c_txd_buffer[4] = (uint8_t)PAYLOAD_OP_FLAGS;
		i2c_txd_buffer[5] = (uint8_t)(PAYLOAD_PRIORITY_DATA_WAITING>>8);
		i2c_txd_buffer[6] = (uint8_t)PAYLOAD_PRIORITY_DATA_WAITING;
		i2c_txd_buffer[7] = (uint8_t)(data_waiting_temp>>24);
		i2c_txd_buffer[8] = (uint8_t)(data_waiting_temp>>16);
		i2c_txd_buffer[9] = (uint8_t)(data_waiting_temp>>8);
		i2c_txd_buffer[10] = (uint8_t)(data_waiting_temp);
		i2c_txd_buffer[11] = PAYLOAD_REQUEST_FLAGS;
		i2c_txd_buffer[12] = 0x00;//PAYLOAD_PARAMETER_1;
		i2c_txd_buffer[13] = 0x00;//PAYLOAD_PARAMETER_2;
		i2c_txd_buffer[14] = 0x00;//PAYLOAD_PARAMETER_3;
		i2c_txd_buffer[15] = 0x00;//PAYLOAD_PARAMETER_4;
		i2c_txd_buffer[16] = 0x00;//PAYLOAD_PARAMETER_5;
		i2c_txd_buffer[17] = 0x00;//PAYLOAD_PARAMETER_6;
		i2c_txd_buffer[18] = 0x00;//PAYLOAD_PARAMETER_7;
		i2c_txd_buffer[19] = 0x00;//PAYLOAD_PARAMETER_8;
		
		// Calculate CRC
		crc = crc16(0xFFFF,i2c_txd_buffer,20);
		i2c_txd_buffer[20] = (uint8_t)(crc>>8);
		i2c_txd_buffer[21] = (uint8_t)crc;
		
		i2c_tx_length = 22;
	}
	I2C_send_packet(i2c_tx_length);
	
	#if OLD_ACK == 1
	I2C_wait_for_crc_ack(crc); // Throw away the ack.
	#else
	I2C_wait_for_ack(0x91);
	#endif
	
}

void pay_ops_update(unsigned int crc){
	#if OLD_ACK == 1
	I2C_send_crc_ack(crc);
	#else
	I2C_send_ack(0x92);
	#endif
	
	uint8_t temp_op_mode = i2c_rxd_buffer[2];
	
	PAYLOAD_OP_FLAGS = i2c_rxd_buffer[3];
	PAYLOAD_OP_FLAGS = PAYLOAD_OP_FLAGS<<8;
	PAYLOAD_OP_FLAGS |= i2c_rxd_buffer[4];
	
	PAYLOAD_PRIORITY_DATA_LIMIT = i2c_rxd_buffer[9];
	PAYLOAD_PRIORITY_DATA_LIMIT = PAYLOAD_PRIORITY_DATA_LIMIT << 8;
	PAYLOAD_PRIORITY_DATA_LIMIT |= i2c_rxd_buffer[10];
	
	PAYLOAD_PRIORITY_DATA_REMAINING = i2c_rxd_buffer[11];
	PAYLOAD_PRIORITY_DATA_REMAINING = PAYLOAD_PRIORITY_DATA_REMAINING << 8;
	PAYLOAD_PRIORITY_DATA_REMAINING |= i2c_rxd_buffer[12];
	
	PAYLOAD_MASS_MEMORY_LIMIT = i2c_rxd_buffer[13];
	PAYLOAD_MASS_MEMORY_LIMIT = PAYLOAD_MASS_MEMORY_LIMIT << 8;
	PAYLOAD_MASS_MEMORY_LIMIT |= i2c_rxd_buffer[14];
	PAYLOAD_MASS_MEMORY_LIMIT = PAYLOAD_MASS_MEMORY_LIMIT << 8;
	PAYLOAD_MASS_MEMORY_LIMIT |= i2c_rxd_buffer[15];
	PAYLOAD_MASS_MEMORY_LIMIT = PAYLOAD_MASS_MEMORY_LIMIT << 8;
	PAYLOAD_MASS_MEMORY_LIMIT |= i2c_rxd_buffer[16];
	
	PAYLOAD_MASS_MEMORY_REMAINING = i2c_rxd_buffer[17];
	PAYLOAD_MASS_MEMORY_REMAINING = PAYLOAD_MASS_MEMORY_REMAINING << 8;
	PAYLOAD_MASS_MEMORY_REMAINING |= i2c_rxd_buffer[18];
	PAYLOAD_MASS_MEMORY_REMAINING = PAYLOAD_MASS_MEMORY_REMAINING << 8;
	PAYLOAD_MASS_MEMORY_REMAINING |= i2c_rxd_buffer[19];
	PAYLOAD_MASS_MEMORY_REMAINING = PAYLOAD_MASS_MEMORY_REMAINING << 8;
	PAYLOAD_MASS_MEMORY_REMAINING |= i2c_rxd_buffer[20];
	
	#if defined(SERIAL_DEBUG)
		Serial.print("New Orbit Memory Value: ");
		Serial.println(INIT_PAYLOAD_MASS_MEMORY_REMAINING - (ORBIT_MAX_MEM * 256), DEC);
	#endif
	
	if(temp_op_mode != PAYLOAD_OP_MODE){
		PAYLOAD_OP_MODE = temp_op_mode;
		if(PAYLOAD_OP_MODE == 1){
			oemv_mode1_set(); // Put GPS into mode 1 - High data rate.
		}else if(PAYLOAD_OP_MODE == 2){
			oemv_mode2_set(); // Put GPS into mode 2 - Low data rate.
		}else if(PAYLOAD_OP_MODE == 0){
		// What should we do here?
		}else{}
		
		bitClear(PAYLOAD_REQUEST_FLAGS,REQUEST_MODE_CHANGE);
	}
        else{
          bitClear(PAYLOAD_REQUEST_FLAGS,REQUEST_UPDATE);
        }
}

void pay_parameter_write(unsigned int crc){
	#if OLD_ACK == 1
	I2C_send_crc_ack(crc);
	#else
	I2C_send_ack(0x93);
	#endif

	unsigned int temp_para;

	switch(i2c_rxd_buffer[2]){
	case 0:
		PAYLOAD_PARAMETER_1 = i2c_rxd_buffer[4];
		break;
	case 1:
		PAYLOAD_PARAMETER_2 = i2c_rxd_buffer[4];
		break;
	case 2:
		temp_para = i2c_rxd_buffer[3];
                temp_para = temp_para << 8;
                temp_para = temp_para|i2c_rxd_buffer[3];
                if(temp_para != ORBIT_MAX_MEM){
                 ORBIT_MAX_MEM = temp_para;
                 #if defined(SERIAL_DEBUG)
                 	Serial.print("New Max Memory Value: ");
                 	Serial.println(ORBIT_MAX_MEM, DEC);
                 #endif
                } 
		break;
	case 3:
		PAYLOAD_PARAMETER_4 = i2c_rxd_buffer[4];
		break;
	case 4:
		PAYLOAD_PARAMETER_5 = i2c_rxd_buffer[4];
		break;
	case 5:
		PAYLOAD_PARAMETER_6 = i2c_rxd_buffer[4];
		break;
	case 6:
		PAYLOAD_PARAMETER_7 = i2c_rxd_buffer[4]; // This is the EEPROM Erase value.
		break;
	case 7:
		PAYLOAD_PARAMETER_8 = i2c_rxd_buffer[4];
		break;
	default:
		break;
	}
}

void pay_parameter_read(){
	unsigned int crc;
	i2c_txd_buffer[0] = 0x94;

	if(ERROR_FLAGS != 0){ // Something has raised an error flag. Send an error packet instead of a status update.
		i2c_txd_buffer[1] = 0x09; // CRC bit is always set, and we have an error, hence 0b1001.
		i2c_txd_buffer[2] = ERROR_FLAGS;
		crc = crc16(0xFFFF,i2c_txd_buffer,3);
		i2c_txd_buffer[3] = (uint8_t)(crc>>8);
		i2c_txd_buffer[4] = (uint8_t)(crc&0xFF);
		i2c_tx_length = 5;
	}else{
		i2c_txd_buffer[1] = 0x01;
		i2c_txd_buffer[2] = i2c_rxd_buffer[2]; //0x00;
		i2c_txd_buffer[3] = 0x00;
		switch(i2c_rxd_buffer[2]){
			case 0:
				i2c_txd_buffer[4] = PAYLOAD_PARAMETER_1;
				break;
			case 1:
				//i2c_txd_buffer[4] = PAYLOAD_PARAMETER_2;
				i2c_txd_buffer[4] = GPS_SATS;
				break;
			case 2:
				i2c_txd_buffer[4] = PAYLOAD_PARAMETER_3;
				break;
			case 3:
				i2c_txd_buffer[4] = PAYLOAD_PARAMETER_4;
				break;
			case 4:
				i2c_txd_buffer[4] = PAYLOAD_PARAMETER_5;
				break;
			case 5:
				i2c_txd_buffer[4] = PAYLOAD_PARAMETER_6;
				break;
			case 6:
				i2c_txd_buffer[4] = PAYLOAD_PARAMETER_7;
				break;
			case 7:
				i2c_txd_buffer[4] = PAYLOAD_PARAMETER_8;
				break;
			defaut:
				break;
		}
		// Calculate CRC
		crc = crc16(0xFFFF,i2c_txd_buffer,5);
		i2c_txd_buffer[5] = (uint8_t)(crc>>8);
		i2c_txd_buffer[6] = (uint8_t)crc;
		i2c_tx_length = 7;
	}
		
	I2C_send_packet(i2c_tx_length);
	
	#if OLD_ACK == 1
	I2C_wait_for_crc_ack(crc); // Throw away the ack.
	#else
	I2C_wait_for_ack(0x94);
	#endif
}

void pay_pri_data_trans(){
	unsigned int crc;
	uint8_t sent_eeprom_packet = 0;
	
	i2c_txd_buffer[0] = 0x95;

	if(ERROR_FLAGS != 0){ // Something has raised an error flag. Send an error packet instead of a status update.
		i2c_txd_buffer[1] = 0x09; // CRC bit is always set, and we have an error, hence 0b1001.
		i2c_txd_buffer[2] = ERROR_FLAGS;
		crc = crc16(0xFFFF,i2c_txd_buffer,3);
		i2c_txd_buffer[3] = (uint8_t)(crc>>8);
		i2c_txd_buffer[4] = (uint8_t)(crc&0xFF);
		i2c_tx_length = 5;
	}else{
		i2c_txd_buffer[1] = 0x01;
		
		// Copy data from EEPROM next page buffer into TX Buffer.
		if( bitRead(EEPROM_FLAGS,EEPROM_PAGE_READY) ){  //EEPROM_read_page() == 1){
			//EEPROM_update_data_waiting();
			for(unsigned int i = 2; i<258; i++){
				i2c_txd_buffer[i] = eeprom_next_read_page[i-2]; //eeprom_buffer[i-2];
			}
			sent_eeprom_packet = 1;

			#if defined(SERIAL_DEBUG)
				Serial.println("EEPROM READ");
			#endif
		}else{ // Or fill TX buffer with zeros.
			// Dummy code to fill buffer with zeros.
			for(unsigned int i = 2; i<258; i++){
				i2c_txd_buffer[i] = 0x00;
			}
		}
		
		
		// Calculate CRC
		crc = crc16(0xFFFF,i2c_txd_buffer,258);
		i2c_txd_buffer[258] = (uint8_t)(crc>>8);
		i2c_txd_buffer[259] = (uint8_t)crc;
		i2c_tx_length = 260;
	}
	
	I2C_send_packet(i2c_tx_length);
	
	uint8_t got_ack = 0;
	#if OLD_ACK == 1
	got_ack = I2C_wait_for_crc_ack(crc);
	#else
	got_ack = I2C_wait_for_ack(0x95);
	#endif
	
	if(got_ack == 1){
		#if defined(EEPROM_ENABLED)
			// Notify main loop to buffer another page
			bitClear(EEPROM_FLAGS,EEPROM_PAGE_READY);
			EEPROM_increment_tail();
			
			if(PAYLOAD_PRIORITY_DATA_WAITING > 0){
				PAYLOAD_PRIORITY_DATA_WAITING--;
			}
			
			// Buffer next page into RAM.
			kick_watchdog();
			EEPROM_buffer_page();
		#endif
	}
}

void pay_data_trans(){
	unsigned int crc;
	uint8_t sent_eeprom_packet = 0;
	
	i2c_txd_buffer[0] = 0x96;

	if(ERROR_FLAGS != 0){ // Something has raised an error flag. Send an error packet instead of a status update.
		i2c_txd_buffer[1] = 0x09; // CRC bit is always set, and we have an error, hence 0b1001.
		i2c_txd_buffer[2] = ERROR_FLAGS;
		crc = crc16(0xFFFF,i2c_txd_buffer,3);
		i2c_txd_buffer[3] = (uint8_t)(crc>>8);
		i2c_txd_buffer[4] = (uint8_t)(crc&0xFF);
		i2c_tx_length = 5;
	}else{
		i2c_txd_buffer[1] = 0x01;
		
		// Copy data from EEPROM next page buffer into TX Buffer.
		if( bitRead(EEPROM_FLAGS,EEPROM_PAGE_READY) ){  //EEPROM_read_page() == 1){
			//EEPROM_update_data_waiting();
			for(unsigned int i = 2; i<258; i++){
				i2c_txd_buffer[i] = eeprom_next_read_page[i-2]; //eeprom_buffer[i-2];
			}
			sent_eeprom_packet = 1;
			
			#if defined(SERIAL_DEBUG)
				Serial.println("EEPROM READ");
			#endif
		}else{ // Or fill TX buffer with zeros.
			// Dummy code to fill buffer with zeros.
			for(unsigned int i = 2; i<258; i++){
				i2c_txd_buffer[i] = 0x00;
			}
			#if defined(SERIAL_DEBUG)
				Serial.println("DUMMY READ");
			#endif
		}
		
		
		// Calculate CRC
		crc = crc16(0xFFFF,i2c_txd_buffer,258);
		i2c_txd_buffer[258] = (uint8_t)(crc>>8);
		i2c_txd_buffer[259] = (uint8_t)crc;
		i2c_tx_length = 260;
	}
	
	I2C_send_packet(i2c_tx_length);
	
	uint8_t got_ack = 0;
	#if OLD_ACK == 1
	got_ack = I2C_wait_for_crc_ack(crc);
	#else
	got_ack = I2C_wait_for_ack(0x96);
	#endif
	
	if(got_ack == 1){
		#if defined(EEPROM_ENABLED)
			// Notify main loop to buffer another page
			bitClear(EEPROM_FLAGS,EEPROM_PAGE_READY);
			EEPROM_increment_tail();
			
			// Buffer next page into RAM.
			kick_watchdog();
			EEPROM_buffer_page();
		#endif
	}
        bitSet(PAYLOAD_REQUEST_FLAGS,REQUEST_UPDATE);
}

void pay_shutdn(unsigned int crc){
	#if OLD_ACK == 1
	I2C_send_crc_ack(crc);
	#else
	I2C_send_ack(0x9F);
	#endif
	
	oemv_shutdown();
	
	#if defined(EEPROM_ENABLED)
		EEPROM_store_state();
	#endif
	
	PAYLOAD_INITIALIZED = 0;
	
	// Wait for death.
}

